/*
 * Main navigation program
 *
 * by Zik Saleeba 2008-04-05
 */

#include <string.h>

#include "ffox.h"
#include "types.h"
#include "std.h"
#include "rc_settings.h"
#include "autopilot.h"
#include "analogin.h"
#include "adc.h"
#include "estimator.h"
#include "fw_h_ctl.h"
#include "fw_v_ctl.h"
#include "infrared.h"
#include "gps.h"
#include "gyro.h"
#include "nav.h"
#include "downlink.h"
#include "ap_downlink.h"
#include "gps_ubx.h"
#include "serial.h"
#include "sensor.h"
#include "servo.h"
#include "dbgprintf.h"


/* defines */
#define CONTROL_LOOP_HZ     100
#define CONTROL_LOOP_TICKS  (1000/portTICK_RATE_MS/CONTROL_LOOP_HZ)

/* prototypes */
static portTASK_FUNCTION_PROTO(Test_Task, pvParameters);
//static portTASK_FUNCTION_PROTO(GPS_Read_Task, pvParameters);
static portTASK_FUNCTION_PROTO(ControlLoop_Task, pvParameters);

static void init_paparazzi(void);
void event_task(void);

/* variables */
/** Define minimal speed for takeoff in m/s */
#define MIN_SPEED_FOR_TAKEOFF 5.

bool_t power_switch;
uint8_t fatal_error_nb = 0;
static const uint16_t version = 1;

uint8_t pprz_mode = PPRZ_MODE_AUTO2;
uint8_t lateral_mode = LATERAL_MODE_MANUAL;

uint8_t vsupply;

bool_t kill_throttle = FALSE;

float slider_1_val, slider_2_val;

bool_t launch = FALSE;

#ifdef MILLIAMP_PER_PERCENT
float energy; /** Fuel consumption */
#endif

bool_t gps_lost = FALSE;


int LeastTimeTaken;
int MostTimeTaken;
int TotalTimeTaken;
int TotalTimeCount;
int ControlCount;
int AnalogCount;
int AltimeterCount;

/*
 * NAME:        Navigate_StartTasks
 * ACTION:      Starts the high-level functions of flight navigation
 * PARAMETERS:  unsigned portBASE_TYPE uxPriority - the priority to
 *                  start the tasks at
 */
 
void Navigate_StartTasks(unsigned portBASE_TYPE uxPriority)
{
    FFox_TaskCreate(ControlLoop_Task, ( const signed portCHAR * const ) "nav ", LARGE_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    FFox_TaskCreate(Test_Task, ( const signed portCHAR * const ) "test", MEDIUM_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
//    xTaskCreate(GPS_Read_Task, ( const signed portCHAR * const ) "gps ", LARGE_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
}



/*
 * NAME:        Test_Task
 * ACTION:      A task which does some testing
 */
 
static portTASK_FUNCTION(Test_Task, pvParameters)
{
    int Magic1 = STACK_MAGIC;
    portTickType PreviousWakeTime;
    int Magic2 = STACK_MAGIC;

    /* The parameters are not used. */
    (void)pvParameters;
    
    LeastTimeTaken = 1000000;
    MostTimeTaken = -1;
    TotalTimeTaken = 0;
    TotalTimeCount = 0;
    ControlCount = 0;
    AnalogCount = 0;
    AltimeterCount = 0;

    /* get the current time */
    PreviousWakeTime = xTaskGetTickCount();
    
    while (TRUE)
    {
        //dbgprintf("%d < %d < %d, %d, %d, %d, ", LeastTimeTaken, (TotalTimeCount == 0) ? 0 : (TotalTimeTaken/TotalTimeCount), MostTimeTaken, ControlCount, AnalogCount, AltimeterCount);
#if 0
        dbgprintd(Sensor_Altitude);
        dbgprintf(",");
        dbgprintd(gps_alt/100.0);
        dbgprintf(",");
        dbgprintd(estimator_z);
        dbgprintf("\r\n");
#endif
        LeastTimeTaken = 1000000;
        MostTimeTaken = -1;
        TotalTimeTaken = 0;
        TotalTimeCount = 0;
        ControlCount = 0;
        AnalogCount = 0;
        AltimeterCount = 0;
            
        /* wait until a second has passed */
        vTaskDelay(1000/portTICK_RATE_MS);
        //vTaskDelayUntil(&PreviousWakeTime, 1000/portTICK_RATE_MS);
        
        if (Magic1 != STACK_MAGIC || Magic2 != STACK_MAGIC)
            dbgprintf("ERROR: test stack corrupted\r\n");
    }
}



/*
 * NAME:        ControlLoop_Task
 * ACTION:      A task which runs the control loop
 */
 
static portTASK_FUNCTION(ControlLoop_Task, pvParameters)
{
    int Magic1 = STACK_MAGIC;
    portTickType PreviousWakeTime;
    portTickType BeforeTime;
    portTickType NowTime;
    int TimeTaken;
    int Magic2 = STACK_MAGIC;
    
    /* The parameters are not used. */
    (void)pvParameters;
    
    dbgprintf("Initialising navigation system\r\n");

    /* initialise the system */
    init_paparazzi();

    /* get the current time */
    PreviousWakeTime = xTaskGetTickCount();
    
    dbgprintf("Starting navigation system\r\n");
    /* control loop */
    while (TRUE)
    {
        BeforeTime = xTaskGetTickCount();
        /* run the control loop once */
        periodic_task();
        
        /* run the event loop once */
        event_task();
        
        /* wait until the control loop period has passed */
        NowTime = xTaskGetTickCount();
        TimeTaken = NowTime - BeforeTime;
        if (TimeTaken < LeastTimeTaken)
            LeastTimeTaken = TimeTaken;
        if (TimeTaken > MostTimeTaken)
            MostTimeTaken = TimeTaken;
        TotalTimeTaken += TimeTaken;
        TotalTimeCount++;
        ControlCount++;
        
        vTaskDelayUntil(&PreviousWakeTime, CONTROL_LOOP_TICKS);
        
        if (Magic1 != STACK_MAGIC || Magic2 != STACK_MAGIC)
            dbgprintf("ERROR: navigation stack corrupted\r\n");
    }
}


/*
 * NAME:        init_paparazzi
 * ACTION:      Initialises the paparazzi navigation code
 */
 
void init_paparazzi()
{
  sys_time_init(); 

  adc_init();

  /************* Sensors initialization ***************/
  ir_init();
  gyro_init();
  gps_init();

  /************ Internal status ***************/
  h_ctl_init();
  v_ctl_init();
  estimator_init();
  alt_kalman_init();
  nav_init();

  gps_configure_uart();

  power_switch = FALSE;
}



/** Define number of message at initialisation */
#define INIT_MSG_NB 2

uint8_t ac_ident = AC_ID;

/** \brief Send a serie of initialisation messages followed by a stream of periodic ones
 *
 * Called at 60Hz.
 */
static inline void reporting_task( void ) {
  static uint8_t boot = TRUE;

  /** initialisation phase during boot */
  if (boot) {
    DOWNLINK_SEND_BOOT(&version);
    boot = FALSE;
  }
  /** then report periodicly */
  else {
    PeriodicSendAp();
  }
}

#ifndef RC_LOST_MODE
#define RC_LOST_MODE PPRZ_MODE_HOME
#endif

#if 0
/** \brief Function to be called when a message from FBW is available */
inline void telecommand_task( void ) {
  uint8_t mode_changed = FALSE;
  copy_from_to_fbw();
  
  uint8_t really_lost = bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL);
  if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch) {
    if  (too_far_from_home) {
      pprz_mode = PPRZ_MODE_HOME;
      mode_changed = TRUE;
    }
    if  (really_lost) {
      pprz_mode = RC_LOST_MODE;
      mode_changed = TRUE;
    }
  }
  if (bit_is_set(fbw_state->status, AVERAGED_CHANNELS_SENT)) {
    bool_t pprz_mode_changed = pprz_mode_update();
    mode_changed |= pprz_mode_changed;
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
    bool_t calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
    rc_settings(calib_mode_changed || pprz_mode_changed);
    mode_changed |= calib_mode_changed;
#endif
  }
  mode_changed |= mcu1_status_update();
  if ( mode_changed )
    PERIODIC_SEND_PPRZ_MODE();

#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
  /** In AUTO1 mode, compute roll setpoint and pitch setpoint from 
   * \a RADIO_ROLL and \a RADIO_PITCH \n
   */
  if (pprz_mode == PPRZ_MODE_AUTO1) {
    /** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
    h_ctl_roll_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0., -AUTO1_MAX_ROLL);
    
    /** Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
    h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0., AUTO1_MAX_PITCH);
  } /** Else asynchronously set by \a h_ctl_course_loop() */

  /** In AUTO1, throttle comes from RADIO_THROTTLE
      In MANUAL, the value is copied to get it in the telemetry */
  if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) {
    v_ctl_throttle_setpoint = fbw_state->channels[RADIO_THROTTLE];
  }
  /** else asynchronously set by v_ctl_climb_loop(); */
  
  mcu1_ppm_cpt = fbw_state->ppm_cpt;
#endif // RADIO_CONTROL


  vsupply = fbw_state->vsupply;
  
  if (!estimator_flight_time) {
    if (pprz_mode == PPRZ_MODE_AUTO2 && fbw_state->channels[RADIO_THROTTLE] > THROTTLE_THRESHOLD_TAKEOFF) {
      launch = TRUE;
    }
  }
}
#endif

/** \fn void navigation_task( void )
 *  \brief Compute desired_course
 */
static void navigation_task( void ) {
#if defined FAILSAFE_DELAY_WITHOUT_GPS
  /** This section is used for the failsafe of GPS */
  static uint8_t last_pprz_mode;

  /** If aircraft is launched and is in autonomus mode, go into
      PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
  if (launch) {
    if (cpu_time_sec - last_gps_msg_t > FAILSAFE_DELAY_WITHOUT_GPS) {
      if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
    last_pprz_mode = pprz_mode;
    pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
    PERIODIC_SEND_PPRZ_MODE();
    gps_lost = TRUE;
      }
    } else /* GPS is ok */ if (gps_lost) {
      /** If aircraft was in failsafe mode, come back in previous mode */
      pprz_mode = last_pprz_mode;
      gps_lost = FALSE;

      PERIODIC_SEND_PPRZ_MODE();
    }
  }
#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
  
  common_nav_periodic_task_4Hz();
  if (pprz_mode == PPRZ_MODE_HOME)
    nav_home();
  else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER)
    nav_without_gps();
  else
    nav_periodic_task();
  
#ifndef PERIOD_NAVIGATION_0 // If not sent periodically (in default 0 mode)
  SEND_NAVIGATION();
#endif

  SEND_CAM();
  
  /* The nav task computes only nav_altitude. However, we are interested
     by desired_altitude (= nav_alt+alt_shift) in any case.
     So we always run the altitude control loop */
  if (v_ctl_mode == V_CTL_MODE_AUTO_ALT)
    v_ctl_altitude_loop();

  if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
            || pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
#ifdef H_CTL_RATE_LOOP
    /* Be sure to be in attitude mode, not roll */
    h_ctl_auto1_rate = FALSE;
#endif
    if (lateral_mode >=LATERAL_MODE_COURSE)
      h_ctl_course_loop(); /* aka compute nav_desired_roll */
    if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
      v_ctl_climb_loop();
    if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE)
      v_ctl_throttle_setpoint = nav_throttle_setpoint;

#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
    v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
    v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
#endif

    h_ctl_pitch_setpoint = nav_pitch;
    Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
    if (kill_throttle || (!estimator_flight_time && !launch))
      v_ctl_throttle_setpoint = 0;
  }
  
#ifdef MILLIAMP_PER_PERCENT
  energy += (float)v_ctl_throttle_setpoint * (MILLIAMP_PER_PERCENT / MAX_PPRZ * 0.25);
#endif
}


/** \fn inline void periodic_task( void )
 *  \brief Do periodic tasks at 60 Hz
 */
/**There are four @@@@@ boucles @@@@@:
 * - 20 Hz:
 *   - lets use \a reporting_task at 60 Hz
 *   - updates ir with \a ir_update
 *   - updates estimator of ir with \a estimator_update_state_infrared
 *   - set \a desired_aileron and \a desired_elevator with \a pid_attitude_loop
 *   - sends to \a fbw \a desired_throttle, \a desired_aileron and
 *     \a desired_elevator \note \a desired_throttle is set upon GPS
 *     message reception
 * - 4 Hz:
 *   - calls \a estimator_propagate_state
 *   - do navigation with \a navigation_task
 *
 */

void periodic_task( void ) 
{
    static uint8_t Tick_4Hz = 0;
    static uint8_t Tick_1Hz = 0;

    void show_ir();

    /* update sensors */    
    gyro_update();
    ir_update();
    estimator_update_state_infrared();
    
    /* increment timers */
    Tick_4Hz++;
    if (Tick_4Hz >= 25) 
        Tick_4Hz = 0;
        
    Tick_1Hz++;
    if (Tick_1Hz >= 100) 
        Tick_1Hz = 0;
    
    reporting_task();
    
    if (!Tick_1Hz) 
    {
        if (estimator_flight_time) 
            estimator_flight_time++;
            
        datalink_time++;
        dbgprintf("+");
    }
    
    switch (Tick_4Hz) 
    {
    case 0:
        estimator_propagate_state();
        navigation_task();
        //dbgprintf("throttle=%-4d, aileron=%-4d, elevator=%-4d\r\n", v_ctl_throttle_slewed, h_ctl_aileron_setpoint, h_ctl_elevator_setpoint);
        show_ir();
        break;
        
    case 1:
        if (!estimator_flight_time && estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) 
        {
            estimator_flight_time = 1;
            launch = TRUE; /* Not set in non auto launch */
            DOWNLINK_SEND_TAKEOFF(&cpu_time_sec);
        }
        break;
    
    case 5:
        // maybe send barometric pressure and temperature here
        //DOWNLINK_SEND_DPICCO_STATUS(&dpicco_val[0], &dpicco_val[1]);
        break;
    
    case 6:
        //DOWNLINK_SEND_ADC_GENERIC(&adc_generic_val1, &adc_generic_val2);
        break;
    
    /*  default: */
    }
    
    h_ctl_attitude_loop(); /* Set  h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
    v_ctl_throttle_slew();

    Servo_Set(COMMAND_THROTTLE, v_ctl_throttle_slewed);
    Servo_Set(COMMAND_ROLL, h_ctl_aileron_setpoint);
    Servo_Set(COMMAND_PITCH, h_ctl_elevator_setpoint);
}


/*********** EVENT ***********************************************************/
void event_task(void)
{
#ifndef HITL /** else comes through the datalink */
    ReadGpsBuffer();
#endif

    if (!gps_configuring && gps_msg_received) 
    {
        /* parse and use GPS messages */
        parse_gps_msg();
        
        gps_msg_received = FALSE;
        if (gps_pos_available) 
        {
            gps_verbose_downlink = !launch;
            UseGpsPos(estimator_update_state_gps);
            gps_pos_available = FALSE;
        }
    }
    
#if 0
    XXX - add these back in later
    if (PprzBuffer()) 
    {
        ReadPprzBuffer();
        if (pprz_msg_received) 
        {
            pprz_parse_payload();
            pprz_msg_received = FALSE;
        }
    }
    
    if (dl_msg_available) 
    {
        dl_parse_msg();
        dl_msg_available = FALSE;
    }
#endif
    
    /* give the altitude reading to the Kalman filter */
    if (Sensor_AltitudeUpdated)
    {
        float baro_z = gps_ground_alt + (Sensor_Altitude - baro_ground_alt);
        EstimatorSetAlt(baro_z);
        Sensor_AltitudeUpdated = FALSE;
    }
}
